//! Simple lidar control test
//!
//! This is a quick test to verify lidar control functionality.

use unitree_ros2_rs::lidar_client::client::{LidarClient, LidarCommand};
use std::thread::sleep;
use std::time::Duration;

fn main() -> Result<(), Box<dyn std::error::Error>> {
    println!("Simple Lidar Control Test");
    
    // Create lidar client
    let lidar_client = LidarClient::new("lidar_test", "")?;
    println!("✓ Lidar client created successfully");
    
    // Test turning on lidar
    println!("\nTesting lidar ON command...");
    lidar_client.turn_on()?;
    println!("✓ ON command sent");
    
    sleep(Duration::from_secs(10));
    
    // Test turning off lidar
    println!("\nTesting lidar OFF command...");
    lidar_client.turn_off()?;
    println!("✓ OFF command sent");
    
    sleep(Duration::from_secs(10));
    
    // Test direct command sending
    println!("\nTesting direct command sending...");
    lidar_client.send_command(LidarCommand::On)?;
    println!("✓ Direct ON command sent");
    println!("✓ Direct OFF command sent");
    
    println!("\n🎉 All lidar control tests completed successfully!");
    println!("\nNote: Commands are published to topic 'rt/utlidar/switch'");
    println!("      Message type: std_msgs::msg::String");
    println!("      Commands: 'ON' to start, 'OFF' to stop lidar");
    
    Ok(())
}